﻿//MIT License
//Copyright (c) 2024 nuaazgg, Nanjing University of Aeronautics and Astronautics(NUAA)

using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using UnityEngine;

public class RobotArm
{
    UInt16 rshd = 0xffff;
    //机械臂IP地址
    string robotIP = "192.168.123.123";
    //机械臂端口号
    int serverPort = 8899;
    Pos headToRobot = new Pos(0, 0, 0.75);
    Quaternion InitOri = new Quaternion();

    const int RSERR_SUCC = 0;
    //关节个数
    const int ARM_DOF = 6;
    //M_PI
    const double M_PI = 3.14159265358979323846;

    //坐标系枚举
    const int BaseCoordinate = 0;
    const int EndCoordinate = 1;
    const int WorldCoordinate = 2;

    //坐标系标定方法
    const int Origin_AnyPointOnPositiveXAxis_AnyPointOnPositiveYAxis = 0; // 原点、x轴正半轴、y轴正半轴
    const int Origin_AnyPointOnPositiveYAxis_AnyPointOnPositiveZAxis = 1; // 原点、y轴正半轴、z轴正半轴
    const int Origin_AnyPointOnPositiveZAxis_AnyPointOnPositiveXAxis = 2; // 原点、z轴正半轴、x轴正半轴
    const int Origin_AnyPointOnPositiveXAxis_AnyPointOnFirstQuadrantOfXOYPlane = 3; // 原点、x轴正半轴、x、y轴平面的第一象限上任意一点
    const int Origin_AnyPointOnPositiveXAxis_AnyPointOnFirstQuadrantOfXOZPlane = 4; // 原点、x轴正半轴、x、z轴平面的第一象限上任意一点
    const int Origin_AnyPointOnPositiveYAxis_AnyPointOnFirstQuadrantOfYOZPlane = 5; // 原点、y轴正半轴、y、z轴平面的第一象限上任意一点
    const int Origin_AnyPointOnPositiveYAxis_AnyPointOnFirstQuadrantOfYOXPlane = 6; // 原点、y轴正半轴、y、x轴平面的第一象限上任意一点
    const int Origin_AnyPointOnPositiveZAxis_AnyPointOnFirstQuadrantOfZOXPlane = 7; // 原点、z轴正半轴、z、x轴平面的第一象限上任意一点
    const int Origin_AnyPointOnPositiveZAxis_AnyPointOnFirstQuadrantOfZOYPlane = 8; // 原点、z轴正半轴、z、y轴平面的第一象限上任意一点
    
    //运动轨迹类型
    //
    //圆
    const int ARC_CIR = 2;
    //圆弧
    const int CARTESIAN_MOVEP = 3;

    //机械臂状态
    const int RobotStopped = 0;
    const int RobotRunning = 1;
    const int RobotPaused = 2;
    const int RobotResumed = 3;

    //机械臂工作模式
    const int RobotModeSimulator = 0; //机械臂仿真模式
    const int RobotModeReal = 1; //机械臂真实模式

    //路点位置信息的表示方法
    [StructLayout(LayoutKind.Sequential)]
    public struct Pos
    {
        public double x;
        public double y;
        public double z;

        public Pos(double xx, double yy, double zz)
        {
            x = xx;
            y = yy;
            z = zz;
        }

        public static Pos operator +(Pos s1, Pos s2)
        {
            return new Pos(s1.x + s2.x, s1.y + s2.y, s1.z + s2.z);

        }
    }


    //路点位置信息的表示方法
    [StructLayout(LayoutKind.Sequential)]
    public struct cartesianPos_U
    {
        // 指定数组尺寸
        [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
        public double[] positionVector;
    };

    //姿态的四元素表示方法
    [StructLayout(LayoutKind.Sequential)]
    public struct Ori
    {
        public double w;
        public double x;
        public double y;
        public double z;

        public Ori(Quaternion rotation)
        {
            w = rotation.w;
            x = rotation.x;
            y = rotation.y;
            z = rotation.z;
        }
    };

    //姿态的欧拉角表示方法
    [StructLayout(LayoutKind.Sequential)]
    public struct Rpy
    {
        public double rx;
        public double ry;
        public double rz;
        public Rpy(double x, double y, double z)
        {
            rx = x;
            ry = y;
            rz = z;
        }
    };

    //描述机械臂的路点信息
    [StructLayout(LayoutKind.Sequential)]
    public struct wayPoint_S
    {
        //机械臂的位置信息　X,Y,Z
        public Pos cartPos;
        //机械臂姿态信息
        public Ori orientation;
        //机械臂关节角信息
        [MarshalAs(UnmanagedType.ByValArray, SizeConst = ARM_DOF)]
        public double[] jointpos;
    };

    //机械臂关节速度加速度信息
    [StructLayout(LayoutKind.Sequential)]
    public struct JointVelcAccParam
    {
        [MarshalAs(UnmanagedType.ByValArray, SizeConst = ARM_DOF)]
        public double[] jointPara;
    };

    //机械臂关节角度
    [StructLayout(LayoutKind.Sequential)]
    public struct JointRadian
    {
        [MarshalAs(UnmanagedType.ByValArray, SizeConst = ARM_DOF)]
        public double[] jointRadian;
    };

    //机械臂工具端参数
    [StructLayout(LayoutKind.Sequential)]
    public struct ToolInEndDesc
    {
        //工具相对于末端坐标系的位置
        public Pos cartPos;
        //工具相对于末端坐标系的姿态
        public Ori orientation;
    };

    //坐标系结构体
    [StructLayout(LayoutKind.Sequential)]
    public struct CoordCalibrate
    {
        //坐标系类型：当coordType==BaseCoordinate或者coordType==EndCoordinate是，下面3个参数不做处理
        public int coordType;
        //坐标系标定方法
        public int methods;
        //用于标定坐标系的３个点（关节角），对应于机械臂法兰盘中心点基于基座标系
        [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
        public JointRadian[] jointPara;
        //标定的时候使用的工具描述
        public ToolInEndDesc toolDesc;
    };

    //转轴定义
    [StructLayout(LayoutKind.Sequential)]
    public struct MoveRotateAxis
    {
        [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
        public double[] rotateAxis;
    };

    //描述运动属性中的偏移属性
    [StructLayout(LayoutKind.Sequential)]
    public struct MoveRelative
    {
        //是否使能偏移
        public byte enable;
        //偏移量 x,y,z
        [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
        public float[] pos;
        //public Pos pos;
        //相对姿态偏移量
        public Ori orientation;
    };

    //该结构体描述工具惯量
    [StructLayout(LayoutKind.Sequential)]
    public struct ToolInertia
    {
        public double xx;
        public double xy;
        public double xz;
        public double yy;
        public double yz;
        public double zz;
    };

    //动力学参数
    [StructLayout(LayoutKind.Sequential)]
    public struct ToolDynamicsParam
    {
        public double positionX; //工具重心的X坐标
        public double positionY; //工具重心的Y坐标
        public double positionZ; //工具重心的Z坐标
        public double payload; //工具重量
        public ToolInertia toolInertia; //工具惯量
    };

    //机械臂事件
    [StructLayout(LayoutKind.Sequential)]
    public struct RobotEventInfo
    {
        public int eventType; //事件类型号
        public int eventCode; //事件代码
        public IntPtr eventContent; //事件内容(std::string)
    };

    //初始化机械臂控制库
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_initialize", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_initialize();

    //反初始化机械臂控制库
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_uninitialize", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_uninitialize();

    //创建机械臂控制上下文句柄
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_create_context", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_create_context(ref UInt16 rshd);

    //注销机械臂控制上下文句柄
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_destory_context", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_destory_context(UInt16 rshd);

    //链接机械臂服务器
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_login", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_login(UInt16 rshd, [MarshalAs(UnmanagedType.LPStr)] string addr, int port);

    //断开机械臂服务器链接
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_logout", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_logout(UInt16 rshd);

    //初始化全局的运动属性
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_init_global_move_profile", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_init_global_move_profile(UInt16 rshd);

    //设置六个关节轴动的最大速度（最大为180度/秒），注意如果没有特殊需求，6个关节尽量配置成一样！
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_global_joint_maxvelc", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_global_joint_maxvelc(UInt16 rshd, double[] max_velc);

    //获取六个关节轴动的最大速度
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_get_global_joint_maxvelc", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_get_global_joint_maxvelc(UInt16 rshd, ref JointVelcAccParam max_velc);

    //设置六个关节轴动的最大加速度 （十倍的最大速度），注意如果没有特殊需求，6个关节尽量配置成一样！
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_global_joint_maxacc", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_global_joint_maxacc(UInt16 rshd, double[] max_acc);

    //获取六个关节轴动的最大加速度
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_get_global_joint_maxacc", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_get_global_joint_maxacc(UInt16 rshd, ref JointVelcAccParam max_acc);

    //设置机械臂末端最大线加速度
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_global_end_max_line_acc", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_global_end_max_line_acc(UInt16 rshd, double max_acc);

    //设置机械臂末端最大线速度
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_global_end_max_line_velc", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_global_end_max_line_velc(UInt16 rshd, double max_velc);

    //获取机械臂末端最大线加速度
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_get_global_end_max_line_acc", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_get_global_end_max_line_acc(UInt16 rshd, ref double max_acc);

    //获取机械臂末端最大线速度
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_get_global_end_max_line_velc", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_get_global_end_max_line_velc(UInt16 rshd, ref double max_velc);

    //设置机械臂末端最大角加速度
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_global_end_max_angle_acc", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_global_end_max_angle_acc(UInt16 rshd, double max_acc);

    //设置机械臂末端最大角速度
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_global_end_max_angle_velc", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_global_end_max_angle_velc(UInt16 rshd, double max_velc);

    //获取机械臂末端最大角加速度
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_get_global_end_max_angle_acc", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_get_global_end_max_angle_acc(UInt16 rshd, ref double max_acc);

    //获取机械臂末端最大角加速度
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_get_global_end_max_angle_velc", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_get_global_end_max_angle_velc(UInt16 rshd, ref double max_velc);

    //设置用户坐标系
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_user_coord", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_user_coord(UInt16 rshd, ref CoordCalibrate user_coord);

    //设置基座坐标系
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_base_coord", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_base_coord(UInt16 rshd);

    //机械臂轴动
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_move_joint", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_move_joint(UInt16 rshd, double[] joint_radia, bool isblock);

    //机械臂直线运动
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_move_line", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_move_line(UInt16 rshd, double[] joint_radia, bool isblock);

    //保持当前位置变换姿态做旋转运动
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_move_rotate", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_move_rotate(UInt16 rshd, ref CoordCalibrate user_coord, ref MoveRotateAxis rotate_axis, double rotate_angle, bool isblock);

    //清除所有已经设置的全局路点
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_remove_all_waypoint", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_remove_all_waypoint(UInt16 rshd);

    //添加全局路点用于轨迹运动
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_add_waypoint", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_add_waypoint(UInt16 rshd, double[] joint_radia);

    //设置交融半径
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_blend_radius", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_blend_radius(UInt16 rshd, double radius);

    //设置圆运动圈数
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_circular_loop_times", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_circular_loop_times(UInt16 rshd, int times);

    //检查用户坐标系参数设置是否合理
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_check_user_coord", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_check_user_coord(UInt16 rshd, ref CoordCalibrate user_coord);

    //设置基于基座标系运动偏移量
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_relative_offset_on_base", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_relative_offset_on_base(UInt16 rshd, ref MoveRelative relative);

    //设置基于用户标系运动偏移量
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_relative_offset_on_user", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_relative_offset_on_user(UInt16 rshd, ref MoveRelative relative, ref CoordCalibrate user_coord);

    //取消提前到位设置
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_no_arrival_ahead", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_no_arrival_ahead(UInt16 rshd);

    //设置距离模式下的提前到位距离
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_arrival_ahead_distance", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_arrival_ahead_distance(UInt16 rshd, double distance);

    //设置时间模式下的提前到位时间
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_arrival_ahead_time", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_arrival_ahead_time(UInt16 rshd, double sec);

    //轨迹运动
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_move_track", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_move_track(UInt16 rshd, int sub_move_mode, bool isblock);

    //保持当前位姿通过直线运动的方式运动到目标位置
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_move_line_to", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_move_line_to(UInt16 rshd, ref Pos target, ref ToolInEndDesc tool, bool isblock);

    //保持当前位姿通过关节运动的方式运动到目标位置
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_move_joint_to", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_move_joint_to(UInt16 rshd, ref Pos target, ref ToolInEndDesc tool, bool isblock);

    //获取机械臂当前位置信息
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_get_current_waypoint", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_get_current_waypoint(UInt16 rshd, ref wayPoint_S waypoint);

    //正解
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_forward_kin", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_forward_kin(UInt16 rshd, double[] joint_radia, ref wayPoint_S waypoint);

    //逆解
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_inverse_kin", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_inverse_kin(UInt16 rshd, double[] joint_radia, ref Pos pos, ref Ori ori, ref wayPoint_S waypoint);

    //四元素转欧拉角
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_rpy_to_quaternion", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_rpy_to_quaternion(UInt16 rshd, ref Rpy rpy, ref Ori ori);

    //欧拉角转四元素
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_quaternion_to_rpy", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_quaternion_to_rpy(UInt16 rshd, ref Ori ori, ref Rpy rpy);

    //基座坐标系转用户坐标系
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_base_to_user", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_base_to_user(UInt16 rshd, ref Pos pos_onbase, ref Ori ori_onbase, ref CoordCalibrate user_coord, ref ToolInEndDesc tool_pos, ref Pos pos_onuser, ref Ori ori_onuser);

    //用户坐标系转基座坐标系
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_user_to_base", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_user_to_base(UInt16 rshd, ref Pos pos_onuser, ref Ori ori_onuser, ref CoordCalibrate user_coord, ref ToolInEndDesc tool_pos, ref Pos pos_onbase, ref Ori ori_onbase);

    //基坐标系转基座标得到工具末端点的位置和姿态
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_base_to_base_additional_tool", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_base_to_base_additional_tool(UInt16 rshd, ref Pos flange_center_pos_onbase, ref Ori flange_center_ori_onbase, ref ToolInEndDesc tool_pos, ref Pos tool_end_pos_onbase, ref Ori tool_end_ori_onbase);

    //设置工具的运动学参数
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_tool_end_param", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_tool_end_param(UInt16 rshd, ref ToolInEndDesc tool);

    //设置无工具的动力学参数
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_none_tool_dynamics_param", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_none_tool_dynamics_param(UInt16 rshd);

    //根据接口板IO类型和地址设置IO状态
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_board_io_status_by_addr", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_board_io_status_by_addr(UInt16 rshd, int io_type, int addr, double val);

    //根据接口板IO类型和地址获取IO状态
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_get_board_io_status_by_addr", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_get_board_io_status_by_addr(UInt16 rshd, int io_type, int addr, ref double val);

    //设置工具端IO状态
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_tool_do_status", CharSet = CharSet.Ansi, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_tool_do_status(UInt16 rshd, string name, int val);

    //获取工具端IO状态
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_get_tool_io_status", CharSet = CharSet.Ansi, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_get_tool_io_status(UInt16 rshd, string name, ref double val);

    //设置工具端电源电压类型
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_tool_power_type", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_tool_power_type(UInt16 rshd, int type);

    //获取工具端电源电压类型
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_get_tool_power_type", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_get_tool_power_type(UInt16 rshd, ref int type);

    //设置工具端数字量IO的类型
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_tool_io_type", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_tool_io_type(UInt16 rshd, int addr, int type);

    //设置工具的动力学参数
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_tool_dynamics_param", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_tool_dynamics_param(UInt16 rshd, ref ToolDynamicsParam tool);

    //获取工具的动力学参数
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_get_tool_dynamics_param", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_get_tool_dynamics_param(UInt16 rshd, ref ToolDynamicsParam tool);

    //设置无工具运动学参数
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_none_tool_kinematics_param", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_none_tool_kinematics_param(UInt16 rshd);

    //设置工具的运动学参数
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_tool_kinematics_param", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_tool_kinematics_param(UInt16 rshd, ref ToolInEndDesc tool);

    //获取工具的运动学参数
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_get_tool_kinematics_param", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_get_tool_kinematics_param(UInt16 rshd, ref ToolInEndDesc tool);

    //启动机械臂
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_robot_startup", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_robot_startup(UInt16 rshd, ref ToolDynamicsParam tool, byte colli_class, bool read_pos, bool static_colli_detect, int board_maxacc, ref int state);

    //关闭机械臂
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_robot_shutdown", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_robot_shutdown(UInt16 rshd);

    //停止机械臂运动
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_move_stop", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_move_stop(UInt16 rshd);

    //停止机械臂运动
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_move_fast_stop", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_move_fast_stop(UInt16 rshd);

    //暂停机械臂运动
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_move_pause", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_move_pause(UInt16 rshd);

    //暂停后回复机械臂运动
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_move_continue", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_move_continue(UInt16 rshd);

    //机械臂碰撞后恢复
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_collision_recover", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_collision_recover(UInt16 rshd);

    //获取机械臂当前状态
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_get_robot_state", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_get_robot_state(UInt16 rshd, ref int state);

    //设置机械臂服务器工作模式
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_work_mode", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_work_mode(UInt16 rshd, int state);

    //获取机械臂服务器当前工作模式
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_get_work_mode", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_get_work_mode(UInt16 rshd, ref int state);

    //设置机械臂碰撞等级
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_set_collision_class", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_set_collision_class(UInt16 rshd, int grade);

    //获取socket链接状态
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_get_socket_status", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_get_socket_status(UInt16 rshd, ref byte connected);

    //设置是否允许实时路点信息推送
    [DllImport("libserviceinterface.dll", EntryPoint = "rs_enable_push_realtime_roadpoint", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
    public static extern int rs_enable_push_realtime_roadpoint(UInt16 rshd, bool enable);

    //实时路点回调函数
    [System.Runtime.InteropServices.UnmanagedFunctionPointerAttribute(System.Runtime.InteropServices.CallingConvention.Cdecl)]
    public delegate void REALTIME_ROADPOINT_CALLBACK(ref wayPoint_S waypoint, IntPtr arg);

    [DllImport("libserviceinterface.dll", CallingConvention = CallingConvention.Cdecl)]
    public static extern void rs_setcallback_realtime_roadpoint(UInt16 rshd, [MarshalAs(UnmanagedType.FunctionPtr)] REALTIME_ROADPOINT_CALLBACK CurrentPositionCallback, IntPtr arg);

    //机械臂事件
    [System.Runtime.InteropServices.UnmanagedFunctionPointerAttribute(System.Runtime.InteropServices.CallingConvention.Cdecl)]
    public delegate void ROBOT_EVENT_CALLBACK(ref RobotEventInfo rs_event, IntPtr arg);

    [DllImport("libserviceinterface.dll", CallingConvention = CallingConvention.Cdecl)]
    public static extern void rs_setcallback_robot_event(UInt16 rshd, [MarshalAs(UnmanagedType.FunctionPtr)] ROBOT_EVENT_CALLBACK RobotEventCallback, IntPtr arg);

    //回调函数
    static void CurrentPositionCallback(ref wayPoint_S waypoint, IntPtr arg)
    {
        PrintWaypoint(waypoint);
    }

    //打印路点信息
    static void PrintWaypoint(wayPoint_S point)
    {
        Debug.LogFormat("---------------------------------------------------------------------------------------");
        Debug.LogFormat("pos.x={0} y={1} z={2}", point.cartPos.x, point.cartPos.y, point.cartPos.z);
        Debug.LogFormat("ori.w={0} x={1} y={2} z={3}", point.orientation.w, point.orientation.x, point.orientation.y, point.orientation.z);
        Debug.LogFormat("joint1={0} joint2={1} joint3={2}", point.jointpos[0] * 180 / M_PI, point.jointpos[1] * 180 / M_PI, point.jointpos[2] * 180 / M_PI);
        Debug.LogFormat("joint4={0} joint5={1} joint6={2}", point.jointpos[3] * 180 / M_PI, point.jointpos[4] * 180 / M_PI, point.jointpos[5] * 180 / M_PI);
        Debug.LogFormat("---------------------------------------------------------------------------------------");
    }

    static void RobotEventCallback(ref RobotEventInfo rs_event, IntPtr arg)
    {
        Debug.LogFormat("---------------------------------------------------------------------------------------");
        Debug.LogFormat("robot event.type={0}", rs_event.eventType);
        Debug.LogFormat("robot event.eventCode={0}", rs_event.eventCode);
        Debug.LogFormat("robot event.eventContent={0}", Marshal.PtrToStringAnsi(rs_event.eventContent));
        Debug.LogFormat("---------------------------------------------------------------------------------------");
    }

    bool robotStartup(UInt16 rshd)
    {
        bool result = false;

        //工具的动力学参数和运动学参数
        ToolDynamicsParam tool_dynamics = new ToolDynamicsParam();
        //机械臂碰撞等级
        byte colli_class = 6;
        //机械臂启动是否读取姿态（默认开启）
        bool read_pos = true;
        //机械臂静态碰撞检测（默认开启）
        bool static_colli_detect = true;
        //机械臂最大加速度（系统自动控制，默认为30000)
        int board_maxacc = 30000;
        //机械臂服务启动状态
        int state = 0;

        if (rs_robot_startup(rshd, ref tool_dynamics, colli_class, read_pos, static_colli_detect, board_maxacc, ref state)
            == RSERR_SUCC)
        {
            result = true;
            Debug.LogFormat("call robot startup succ, robot state:{0}",state);
        }
        else
        {
            Debug.LogFormat("robot startup failed");
        }

        return result;
    }

    public RobotArm(string IP,int port)
    {
        robotIP = IP;
        serverPort = port;
        int result = 0xffff;
        double[] target0 = { 0, 0, -90.0 / 180.0 * M_PI, -90.0 / 180.0 * M_PI, -90.0 / 180.0 * M_PI, 0}; //正立//注意这个里面的值是弧度！
        double[] target1 = { -90.0 / 180.0 * M_PI, 0, -90.0 / 180.0 * M_PI, -90.0 / 180.0 * M_PI, -90.0 / 180.0 * M_PI, 0 };
        //double[] target0 = { 0, 60 / 180.0 * M_PI, -70.0 / 180.0 * M_PI, -100.0 / 180.0 * M_PI, -90.0 / 180.0 * M_PI, 0 }; //侧立//注意这个里面的值是弧度！
        //double[] max_velc = { 1.5, 1.5, 1.5, 1.5, 1.5, 1.5 };
        double[] max_velc = { 0.5,0.5,0.5,0.5,0.5,0.5 };
        double[] max_acc = { 5, 5, 5, 5, 5, 5 };
        //JointVelcAccParam temp = new JointVelcAccParam();
        wayPoint_S waypoint = new wayPoint_S();
        Debug.LogFormat("call rs_initialize");

        //初始化机械臂控制库
        result = rs_initialize();
        Debug.LogFormat("rs_initialize.ret={0}", result);
        if (RSERR_SUCC == result)
        {
            //创建机械臂控制上下文句柄
            if (rs_create_context(ref rshd) == RSERR_SUCC)
            {
                Debug.LogFormat("rshd={0}", rshd);
                //机械臂事件回调
                    ROBOT_EVENT_CALLBACK RobotEventCallbackPtr = new ROBOT_EVENT_CALLBACK(RobotEventCallback);
                    rs_setcallback_robot_event(rshd, RobotEventCallbackPtr, IntPtr.Zero);
                //链接机械臂服务器
                if (rs_login(rshd, robotIP, serverPort) == RSERR_SUCC)
                {
                    Debug.LogFormat("login succ.");
                    robotStartup(rshd);

                    rs_move_joint(rshd, target0, true);
                    /*rs_move_joint(rshd, target1, true);
                    rs_move_joint(rshd, target0, true);
                    rs_move_joint(rshd, target1, true);*/

                }
                rs_set_collision_class(rshd, 10);
                //直接获取机械臂当前位置信息
                rs_get_current_waypoint(rshd, ref waypoint);
                //打印路点信息
                PrintWaypoint(waypoint);
                InitOri = new Quaternion((float)waypoint.orientation.x, (float)waypoint.orientation.y, (float)waypoint.orientation.z, (float)waypoint.orientation.w);
                rs_init_global_move_profile(rshd);
                rs_set_global_joint_maxacc (rshd, max_acc);
                //设置六个关节轴动的最大速度
                rs_set_global_joint_maxvelc (rshd, max_velc);

            }
        }
    }
    ~RobotArm()
    {
        rs_logout(rshd);
        rs_destory_context(rshd);
        rs_uninitialize();
    }

    public double[] SetAngle(Vector3 position, Quaternion rotation,bool isblock)
    {
        //TODO 限位
        var pos = new Pos(- Mathf.Clamp(position.z,0.1f,0.5f), Mathf.Clamp(position.x,-0.3f,0.3f), Mathf.Clamp(position.y,-0.4f,0.1f)) + headToRobot; //正立
        //var pos = new Pos(-position.z, -position.y, position.x);// + headToRobot; //侧立
        //Debug.Log(InitOri.eulerAngles.x + "," + InitOri.eulerAngles.y + "," + InitOri.eulerAngles.z);
        //Debug.Log(rotation.eulerAngles.x  + "," + rotation.eulerAngles.y+ "," + rotation.eulerAngles.z);
        var tempR = rotation.eulerAngles;
        rotation = Quaternion.Euler(tempR.z, -tempR.x, -tempR.y);
        rotation = rotation * InitOri;
        //Debug.Log(rotation.eulerAngles.x + "," + rotation.eulerAngles.y + "," + rotation.eulerAngles.z);
        Ori ori = new Ori(rotation);
        var currentWayPoint = new wayPoint_S();
        var outWayPoint = new wayPoint_S();
        //rs_login(rshd, robotIP, serverPort);
        //Thread.Sleep(2);
        rs_get_current_waypoint(rshd, ref currentWayPoint);
        rs_inverse_kin(rshd, currentWayPoint.jointpos, ref pos, ref ori, ref outWayPoint);
/*        var rpy = new Rpy();
        rs_quaternion_to_rpy(rshd, ref outWayPoint.orientation, ref rpy);
        Debug.Log(rpy.rx/M_PI*180 + "," + rpy.ry / M_PI * 180 + "," + rpy.rz / M_PI * 180);*/
        rs_move_joint(rshd, outWayPoint.jointpos, isblock);
        return outWayPoint.jointpos;
    }

    public double[] GetAngle()
    {
        var outWayPoint = new wayPoint_S();
        var temp = new Rpy();
        //rs_login(rshd, robotIP, serverPort);
        rs_get_current_waypoint(rshd, ref outWayPoint);
        rs_quaternion_to_rpy(rshd,ref outWayPoint.orientation,ref temp);
        Debug.Log(outWayPoint.orientation.x + "," + outWayPoint.orientation.y + "," + outWayPoint.orientation.z+","+outWayPoint.orientation.w);
        Debug.Log(temp.rx*180/M_PI+","+temp.ry * 180 / M_PI+","+temp.rz * 180 / M_PI);
        return outWayPoint.jointpos;
    }
    public void Stop()
    {
        rs_logout(rshd);
        rs_destory_context(rshd);
        rs_uninitialize();
        
    }
}
